#include "motor.h"



/**
 * @brief 硬件映射，请根据主板引脚和drivers_pwm.h里的枚举修改
 * 
 */

#define MOTOR1_DIR C9
#define MOTOR1_PWM PWM2_MODULE1_CHA_C8

#define MOTOR2_DIR C7
#define MOTOR2_PWM PWM2_MODULE0_CHA_C6


#define SERVO_PWM PWM2_MODULE2_CHA_C10
#define SERVO_MID_DUTY 735 // 暂时测得舵机中值为735

// 电机速度限制
#define speed_limit 9000


/**
 * @brief 左右电机初始化
 * 
 */
void motor_init(void)
{
    gpio_init(MOTOR1_DIR, GPO, GPIO_HIGH,GPO_PUSH_PULL); // 电机1方向引脚初始化
    pwm_init(MOTOR1_PWM, 17000, 0); // 电机1PWM引脚初始化，频率17KHz，初始占空比0

    gpio_init(MOTOR2_DIR, GPO, GPIO_HIGH, GPO_PUSH_PULL); // 电机2方向引脚初始化
    pwm_init(MOTOR2_PWM, 17000, 0);                       // 电机2PWM引脚初始化，频率17KHz，初始占空比0
}

/**
 * @brief 舵机初始化
 * 
 */
void servo_init(void)
{
    pwm_init(SERVO_PWM, 17000, 0);
}


/**
 * @brief 设置左电机速度
 * 
 * @param 占空比 
 */
void motor1_set_speed(int left_duty)
{
    if (left_duty > 0) // 前进
    {
        if (left_duty > speed_limit) left_duty = speed_limit; // 限制最大速度
        gpio_set_level(MOTOR1_DIR, GPIO_HIGH);
        pwm_set_duty(MOTOR1_PWM, left_duty);
    }
    else if (left_duty < 0) // 后退
    {
        if (left_duty < -speed_limit)left_duty = -speed_limit; // 限制最大速度
        gpio_set_level(MOTOR1_DIR, GPIO_LOW);
        pwm_set_duty(MOTOR1_PWM, -left_duty);
    }
    else // 停止
    {
        pwm_set_duty(MOTOR1_PWM, 0);
    }
}



/**
 * @brief 设置右电机速度
 *
 * @param 占空比
 */
void motor2_set_speed(int right_duty)
{
    if (right_duty > 0) // 前进
    {
        if (right_duty > speed_limit)right_duty = speed_limit; // 限制最大速度
        gpio_set_level(MOTOR2_DIR, GPIO_HIGH);
        pwm_set_duty(MOTOR2_PWM, right_duty);
    }
    else if (right_duty < 0) // 后退
    {
        if (right_duty < -speed_limit)right_duty = -speed_limit; // 限制最大速度
        gpio_set_level(MOTOR2_DIR, GPIO_LOW);
        pwm_set_duty(MOTOR2_PWM, -right_duty);
    }
    else // 停止
    {
        pwm_set_duty(MOTOR2_PWM, 0);
    }
}


/**
 * @brief 设置全部电机的速度
 * 
 * @param left_speed 
 * @param right_speed 
 */
void set_speed(int left_duty, int right_duty)
{
    motor1_set_speed(left_duty);
    motor2_set_speed(right_duty);
}

void set_servo_speed(int servo_duty)
{
    pwm_set_duty(SERVO_PWM, SERVO_MID_DUTY + servo_duty);
}

